import time
import redis
import minimalmodbus as modbus


class Wheel(object):
    def __init__(self, _port, _slaveaddress, p_setting_speed, p_setting_direction, p_forward_direction,
                 p_backward_direction, function_code):
        self.slave = modbus.Instrument(port=_port, slaveaddress=_slaveaddress)
        self.slave. debug = False
        self.slave.serial.timeout = 0.1
        self.slave.serial.baudrate = 9600
        self.function_code = function_code
        # self.slave.write_register(0x00B6, 0x0001, 0, self.function_code)
        self.slave.write_register(0x4657, 1, 0, self.function_code)

        self.p_setting_speed = p_setting_speed
        self.p_setting_direction = p_setting_direction
        self.p_forward_direction = p_forward_direction
        self.p_backward_direction = p_backward_direction

        self.current_speed = 0

        self.is_forward = False
        self.is_backward = False

        self.scale = 800

    def forward(self, running_speed=None):
        try:
            if running_speed:
                speed = running_speed
            else:
                speed = self.current_speed + self.scale

            print(speed)

            if not self.is_forward:
                self.slave.write_register(self.p_setting_speed, speed, 0, self.function_code)
                self.slave.write_register(self.p_setting_direction, self.p_forward_direction, 0, self.function_code)
                self.is_forward = True
            else:
                if self.current_speed == speed:
                    pass
                else:
                    self.current_speed = speed
                    self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)
        except:
            pass

    def slow_down(self):
        try:
            self.current_speed = self.current_speed - self.scale
            if self.current_speed < 0:
                self.current_speed = 0

            if self.is_forward or self.is_backward:
                self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)

            if self.current_speed == 0:
                self.is_forward = False
                self.is_backward = False
        except:
            pass

    def stop(self):
        try:
            self.is_forward = False
            self.is_backward = False
            self.current_speed = 0

            self.slave.write_register(self.p_setting_speed, 0, 0, self.function_code)
        except:
            pass

    def backward(self, running_speed=None):
        try:
            if running_speed:
                speed = running_speed
            else:
                speed = self.current_speed + self.scale

            if not self.is_backward:
                self.slave.write_register(self.p_setting_speed, speed, 0, self.function_code)
                self.slave.write_register(self.p_setting_direction, self.p_backward_direction, 0, self.function_code)
                self.is_backward = True
            else:
                if self.current_speed == speed:
                    pass
                else:
                    self.current_speed = speed
                    self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)
        except:
            pass


class Drive(object):
    def __init__(self, left, right):
        self.left = left
        self.right = right
        self.r = redis.Redis(db=0)
        self.new_job = False

        self.task_index = 1
        self.last_oy = 0

    def forward_speed_up(self, speed_left=None, speed_right=None):
        print('forward')
        if self.left.is_backward or self.right.is_backward:
            self.speed_down()
        else:
            self.left.forward(speed_left)
            self.right.forward(speed_right)

    def speed_down(self):
        self.left.slow_down()
        self.right.slow_down()

    def go_right(self):
        if self.left.current_speed < self.right.current_speed:
            if self.left.is_forward or self.right.is_forward:
                self.left.forward()
            else:
                self.left.backward()
        else:
            self.right.slow_down()

    def go_left(self):
        if self.right.current_speed < self.left.current_speed:
            if self.left.is_forward or self.right.is_forward:
                self.right.forward()
            else:
                self.right.backward()
        else:
            self.left.slow_down()

    def backward_speed_up(self, speed_left=None, speed_right=None):
        if self.left.is_forward or self.right.is_forward:
            self.speed_down()
        else:
            self.left.backward(speed_left)
            self.right.backward(speed_right)

    def rotate_right(self):
        self.left.forward()
        self.right.backward()

    def rotate_left(self):
        self.left.backward()
        self.right.forward()

    def rotate_over_fix_speed(self, speed_left=None, speed_right=None):
        if speed_right > 0:
            self.right.forward(speed_right)
        else:
            self.right.backward(-speed_right)

        if speed_left > 0:
            self.left.forward(speed_left)
        else:
            self.left.backward(-speed_left)

    def stop(self):
        self.left.stop()
        self.right.stop()


class TestDrive(Drive):
    def __init__(self):
        self.current_speed = 0

    def forward_speed_up(self, speed_left=None, speed_right=None):
        pass
ao
    def speed_down(self):
        pass

    def go_right(self):
        pass

    def go_left(self):
        pass

    def backward_speed_up(self, speed_left=None, speed_right=None):
        pass

    def rotate_right(self):
        pass

    def rotate_left(self):
        pass

    def stop(self):
        pass

    def rotate_over_fix_speed(self,  speed_left=None, speed_right=None):
        print('rotate_over_fix_speed')


if __name__ == '__main__':
    r = redis.Redis(db=0)
    server_r = redis.Redis(host='117.185.46.193', password='lwj380279011', db=0)

 ao    # left = Wheel('/dev/ttyUSB0', 2, 0x4420, 0x466D, 1, 0, 16)
    # right = Wheel('/dev/ttyUSB0', 1, 0x4420, 0x466D, 0, 1, 16)
    #

    left = Wheel('/dev/ttyUSB1', 1, 0x4420, 0x466D, 1, 0, 16)
    right = Wheel('/dev/ttyUSB1', 2, 0x4420, 0x466D, 0, 1, 16)

    # left = Wheel('/dev/ttyUSB0', 1, 0x0056, 0x0066, 2, 1, 6)
    # right = Wheel('/dev/ttyUSB0', 2, 0x0056, 0x0066, 1, 2, 6)

    autopilot = Drive(left, right)

    # autopilot = TestDrive()

    auto = False
    joystick = False
    tv_control = True

    r.set('auto', 'off')
    r.set('keyboard_input', 0)

    while True:
        key = r.get('keyboard_input').decode('ascii')

        if tv_control:
            user_move_timestamp = int(r.get('user_move_timestamp'))

            if time.time() - user_move_timestamp > 2:
                print('stop')
                # autopilot.forward_speed_up(0, 0)
                autopilot.speed_down()

                r.set('forward_left_speed', 0)
                r.set('forward_right_speed', 0)

                # joystick = False
                auto = False

        if key == 'o':
            print('tv_control_off')
            tv_control = False

            r.set('keyboard_input', 0)
        elif key != '0':
            # tv_control = False
            auto = False
            joystick = False

            r.set('auto', 'off')

            if key == 'Key.up':  # up
                print('forward')
                autopilot.forward_speed_up()
            elif key == 'Key.left': # left
                print('left')
                autopilot.go_left()
            elif key == 'Key.right':    # right
                print('right')
                autopilot.go_right()
            elif key == 'Key.down':    # down
                 print('back')
                autopilot.backward_speed_up()
            elif key == 'Key.space':    # space
                print('stop')
                autopilot.stop()
            elif key == 'q':     # q
                print('rotate left')
                autopilot.rotate_left()
            elif key == 'e':     # e
                print('rotate right')
                autopilot.rotate_right()
            elif key == 'a':     # a
                print('auto')
                auto = True
                tv_control = True
                r.set('auto', 'on')
            elif key == 'r':
                print('remote')
                joystick = True
                tv_control = True
            elif key == 't':
                print('teamviewer control')
                tv_control = True
            elif key == 'g':
                print('record gps point')
                print('%s %s' % (r.get('lat').decode("utf-8"), r.get('lon').decode("utf-8")))

            r.set('keyboard_input', 0)
        elif auto:
            try:
                server_r.get('job_index')

                speed_left = int(r.get('forward_left_speed'))
                speed_right = int(r.get('forward_right_speed'))

                if speed_left == 0 or speed_right == 0:
                    autopilot.speed_down()
                    autopilot.speed_down()
                    autopilot.speed_down()
                    autopilot.speed_down()

                    autopilot.stop()
                elif speed_left * speed_right < 0:
                    autopilot.rotate_over_fix_speed(speed_left, speed_right)
                else:
                    autopilot.forward_speed_up(speed_left=speed_left, speed_right=speed_right)
                
            except redis.exceptions.ConnectionError:
                print('disconnect')
                speed_right = 0
                speed_left = 0

                autopilot.stop()
        elif joystick:
            speed_left = int(r.get('forward_left_speed'))
            speed_right = int(r.get('forward_right_speed'))

            print("{0} {1}".format(speed_left, speed_right))

            if speed_left == 0 or speed_right == 0:
                autopilot.stop()
            elif speed_left*speed_right < 0:
                autopilot.rotate_over_fix_speed(speed_left, speed_right)
            else:
                autopilot.forward_speed_up(speed_left=speed_left, speed_right=speed_right)

            # time.sleep(0.2)
        else:
            # time.sleep(0.2)
            pass

        time.sleep(0.1)
